/*************************************************
 *  Object:    Tricopter PID Control (Sensors)
 *  Date:      26/06/2011 (First)
 *             07/07/2011 (Last
 *  Authors:   Teo, Carletto
 *************************************************/

#define TABLE_DIM       16
#define LED             PORTC.B2
#define LED_DIR         TRISC.B2



/* Smarty function to send on RS232 a char String
 */
void Soft_UART_Write_Text(char* testo) {
  char cnt;
  for (cnt=0; cnt<strlen(testo); cnt++) {
      Soft_UART_Write(testo[cnt]);
  }
}

/* Write a Byte of data to an address of ADXL345
 */
void ADXL345_Wr(unsigned short address, unsigned short in) {
    I2C1_Start();
    I2C1_Wr(0x3A);      // Write mode
    I2C1_Wr(address);
    I2C1_Wr(in);
    I2C1_Stop();
}

/* Read a Byte of data from an address of ADXL345
 */
unsigned short ADXL345_Rd(unsigned short address) {
    unsigned short out;
    I2C1_Start();
    I2C1_Wr(0x3A);      // Write mode
    I2C1_Wr(address);
    I2C1_Stop();
    I2C1_Start();
    I2C1_Wr(0x3B);      // Read Mode
    out = I2C1_Rd(0);
    I2C1_Stop();
    return out;
}

/* Fetch acceleration data from ADXL345
 *   NOTE: Address of 3 integer array
 *   must be provided!
 */
void ADXL345_getAcc(int *acc) {
    unsigned short cnt;
    unsigned short acc_tmp[6];
    char z_string[8];

    I2C1_Start();
    I2C1_Wr(0x3A);      // Write mode
    I2C1_Wr(0x32);
    I2C1_Stop();
    I2C1_Start();
    I2C1_Wr(0x3B);      // Read Mode
    for (cnt=0; cnt<6; cnt++) {
        char ack;
        ack = (cnt<5);
        acc_tmp[cnt] = I2C1_Rd(ack);
    }
    I2C1_Stop();
    
    acc[0] = acc_tmp[1];
    acc[0] = acc[0] << 8;
    acc[0] |= acc_tmp[0];
    acc[1] = acc_tmp[3];
    acc[1] = acc[1] << 8;
    acc[1] |= acc_tmp[2];
    acc[2] = acc_tmp[5];
    acc[2] = acc[2] << 8;
    acc[2] |= acc_tmp[4];
}

/* Initial fetch of all values for the comparing table
 */
void dataArrayInit(int *fifox, int *fifoy, int *fifoz) {
     unsigned short cnt;
     int acc_tmp[3];
     for (cnt=0; cnt<TABLE_DIM; cnt++) {
         ADXL345_getAcc(&acc_tmp);
         fifox[cnt] = acc_tmp[0];
         fifoy[cnt] = acc_tmp[1];
         fifoz[cnt] = acc_tmp[2];
     }
}

/* Update the table with a newer value deleting the old one
 */
void dataArrayUpdate(int *fifox, int *fifoy, int *fifoz) {
     static unsigned short table_pointer;
     int acc_tmp[3];
     if (table_pointer < 0 || table_pointer >= TABLE_DIM) table_pointer = 0;
     ADXL345_getAcc(&acc_tmp);
     fifox[table_pointer] = acc_tmp[0];
     fifoy[table_pointer] = acc_tmp[1];
     fifoz[table_pointer] = acc_tmp[2];
     table_pointer++;
}

/* Filtering the axis value with comparing table
 */
int dataArrayFilter(int *fifo) {
     unsigned short cnt;
     long value = 0;
     for (cnt=0; cnt<TABLE_DIM; cnt++) {
         value += fifo[cnt];
     }
     return (int)(value/TABLE_DIM);
}


void main() {

    int acc_now[3];        // Accelerazione istantanea (0->X; 1->Y; 2->Z)
    int acc_x[TABLE_DIM];  // Array di accelerazioni asse X
    int acc_y[TABLE_DIM];  // Array di accelerazioni asse Y
    int acc_z[TABLE_DIM];  // Array di accelerazioni asse Z

    char x_string[8];
    char y_string[8];
    char z_string[8];

    LED_DIR = 0;      // LED is set as OUTPUT

    ADCON1 = 0x0F;    // Set A/D Disabled
    CMCON = 0xCF;     // Set Comparator Disabled

    Soft_UART_Init(&PORTB, 6, 7, 38400, 0);
    I2C1_Init(400000);
    Delay_ms(500);

    ADXL345_Wr(0x2C, 0x0B);      // 200Hz conversion data rate
    ADXL345_Wr(0x2D, 0x08);      // Measuring activated ;)
    ADXL345_Wr(0x31, 0x0B);      // Resolution set to +/- 16g (WOW!)
    Delay_ms(500);
    
    dataArrayInit(&acc_x, &acc_y, &acc_z);

    while (1) {
           LED = !LED;

           dataArrayUpdate(&acc_x, &acc_y, &acc_z);
           
           IntToStr(dataArrayFilter(&acc_x), x_string);
           IntToStr(dataArrayFilter(&acc_y), y_string);
           IntToStr(dataArrayFilter(&acc_z), z_string);

           Soft_UART_Write_Text("X: ");
           Soft_UART_Write_Text(&x_string);
           Soft_UART_Write_Text(" - Y: ");
           Soft_UART_Write_Text(&y_string);
           Soft_UART_Write_Text(" - Z: ");
           Soft_UART_Write_Text(&z_string);
           Soft_UART_Write(13);
           Soft_UART_Write(10);
           
           Delay_ms(50);
    }


}